Robot device operation control device and operation control method

ABSTRACT

A ZMP equilibrium equation stating the relationship of various moments applied to a robot body of a robot, based on desirable motion data made up by trajectories of respective parts, imaginarily divided from the robot body, is generated, and moment errors in a ZMP equilibrium equation are calculated. A priority sequence of the parts, the target trajectories of which are corrected to cancel out the moment errors, is set. The target trajectories are corrected from one part to another, in a sequence corresponding to the priority sequence, to compensate the moment errors.

TECHNICAL FIELD

This invention relates to a motion controlling apparatus and a motioncontrolling method for a robot apparatus having at least plural mobileparts, and to a computer program. More particularly, this inventionrelates to a motion controlling apparatus and a motion controllingmethod for a mobile robot apparatus having mobile legs, in which posturestabilizing control is performed using ZMP as a criterion for verifyingthe stability, and to a computer program.

Still more particularly, the present invention relates to a motioncontrolling apparatus and a motion controlling method for a robotapparatus having at least plural mobile parts, and to a computerprogram, in which the target (desirable) trajectories for respectiveparts of the robot body are corrected to compensate a moment of anunknown external force and an unknown external force.

The present application claims priority of Japanese Patent ApplicationNo. 2002-297206, filed in Japan on Oct. 10, 2002, the entirety of whichis incorporated by reference herein.

BACKGROUND ART

A mechanical apparatus for performing movements like those of the humanbeing, using electrical or magnetic operations, is termed a “robot”. Theetymology of the term robot is said to be “ROBOTA” (slave machine) ofthe Slavonic language. The robot started to be used in this nationextensively in later sixties. Most of the robots used were industrialrobots, such as manipulator and a transport robot, aimed to automate theproduction operations in plants or to perform unmanned operations inplants.

In recent years, developments of utility robots, supporting the humanlife as a partner to the human being, that is, supporting the humanactivities in various aspects in our everyday life, are progressing. Indistinction from a crawler type robot, or a robot walking on four or sixlegs, the robot erected and walking on two legs is labile andexperiences increasing difficulties in posture or walking control.However, the robot erected and walking on two legs is superior in beingable to realize flexible motion operations, such as by coping withwalking surfaces presenting protuberances or recesses, as when the robotwalks on an untrimmed terrain, with walking surfaces presentingobstacles on a working route, or with non-continuous walking surfaces,as when he robot has to go up or down a stairway or a ladder.

The legged mobile robot, simulating the living mechanism or movements ofthe human being, is termed a “humanoid” or a “humanoid robot”. Thehumanoid robot is able to support the human life, that is, able tosupport human activities in various aspects in our everyday life, suchas in our living environment.

The working space or the living space of the human being is mostlyformed to the bodily mechanism and action patterns proper to the humanbeing, erected and walking on two legs, while presenting many obstaclesto the motion of the mechanical system of the current state of the art,having wheels or other driving devices as motion means. Hence, if suchmechanical system, that is, a robot, is to perform various humanoperations to take the place of the human being and to adapt itself toour living space, it is desirable that the range of possible motion ofthe robot is approximately equal to that of the human being. Thisaccounts for general expectations entertained for putting the leggedmobile into practical use.

A great variety of technologies pertinent to posture control or stablewalking of the two-legged mobile robot have already been proposed. Thestable “walking” may be defined as “motion on legs without falldown”.The stable posture control for a robot is crucial for avoiding thefalldown of the robot because the falldown means interruption of theoperations being carried out by the robot and considerable labor andtime are needed for the robot to rise from the falldown state toreinitiate the operations. Moreover, such falldown is likely to inflictfatal damages to the robot itself or to a counterpart object againstwhich the robot collides in the course of falldown thereof.

The majority of the proposals for stable posture control of the robotand prevention of falldown during walking use the ZMP (zero momentpoint) as the criterion for verifying the walking stability. Thecriterion for verifying the stability by ZMP is based on the ‘D'Aembert's principle’ that the force of gravity, the force of inertia andthe moment thereof in a direction from the floor surface to the walkingsystem, counterbalance the force of reaction from the floor and themoment thereof in a direction from the floor surface to the walkingsystem. As a conclusion of the mechanical inference, there is a point ofzero pitch axis moment and zero roll axis moment inwardly of a supportpolygon defined by the floor contact point of the foot sole and thefloor surface, that is, a ZMP stable area (see, for example, MiomirVukobratovic, “Legged Locomotive Robots” (translated by Ichiro KATO etal., “Walking Robot and Artificial Leg”, published by NIKKAN KOGYOSHIMBUN-SHA).

In sum, the ZMP criterion is that if, in all instants of walking, theZMP exists inwardly of a support polygon defined by the foot and thefloor surface, and there acts a force in a direction the robot thruststhe floor surface, the robot can walk in stability without falldown(i.e. without the robot body performing rotating motion).

The generation of a two-legged walking pattern, based on the ZMPcriterion, has a merit that the touchdown point of the foot sole can bepreviously set and that account may readily be taken of kinematicconstraint conditions of the foot sole in keeping with the floor surfaceprofile. Moreover, using the ZMP as the criterion for verifying thestability means handling the trajectory rather than the force as atarget value for kinematic control, and hence raises technicalfeasibility.

For example, the legged mobile robot is able to perform stabilizedwalking in such a manner that a point on a floor surface correspondingto the zero ZMP coincides with a target value (see, for example, theJapanese Laid-Open patent Publication H-5-305579).

Moreover, the legged mobile robot may be constructed so that the ZMPwill be inside a support polyhedron (polygon) or so that the ZMP will beat a distance with a certain allowance from an end part of the supportpolygon at the time of touchdown or clearing the floor (see for examplethe Japanese Laid-Open patent Publication H-5-305581). In this case,there is an allowance of the ZMP by a certain preset distance, evenunder occurrence of interference, thus improving stability of the robotbody during walking.

The walking speed of the legged mobile robot may also be controlled by aZMP target position (see, for example, the Japanese Laid-Open PatentPublication H-5-305583). That is, pre-set walking pattern data are used,and the joints of the leg parts are driven so that the ZMP coincideswith a target position. The tilt of the upper body is detected and thewalking pattern data emitting rate is changed depending on the detectedvalue. In case the robot treads on an unknown irregular surface and thusleans forward, the emitting rate is quickened to recover the posture.Since the ZMP is controlled to the target position, the emitting ratemay be changed without any inconvenience if the emitting rate is changedduring the time both leg units of the robot are in the stance position.

On the other hand, the touchdown position of the legged mobile robot maybe controlled by the ZMP target position (see for example the JapaneseLaid-Open patent Publication H-5-305585). That is, in the legged mobilerobot, described in this Patent Publication, the deviation between theZMP target position and the actually measured position is detected. Inorder to eliminate this deviation, one or both of the leg units aredriven, or the moment about the ZMP target position is detected and theleg units are actuated to reduce the moment to zero such as to realizestable walking.

The leaning posture of the legged mobile robot may also be controlled bythe ZMP target position (see for example the Japanese Laid-Open patentPublication H-5-305586). That is, the moment about the ZMP targetposition is detected and, if such moment is produced, the leg units aredriven such as to reduce the moment to zero, in order to effect stablewalking.

The posture stabilizing control of the robot, employing the ZMP as thecriterion for verifying the stability, resides in searching a point ofzero pitch axis moment and zero roll axis moment within a supportpolygon defined by the touchdown point of the foot sole and the floorsurface.

However, as a result of the a priori validation by the presentinventors, it has become clear that, when the robot performs high-speedleg movements, not only the moments about the pitch axis and about theroll axis of the robot body, but also the moment about its yaw axis, isproduced.

FIG. 1 shows an illustrative relationship between the walking speed[second/step] of the two-legged mobile robot and the moment [Nm]generated in the yaw axis direction. As may be seen from this figure,the shorter the time needed per step of the legged mobile robot, thatis, the higher the walking speed, the larger becomes the moment aboutthe yaw axis to a marked extent.

Such moment about the yaw axis produces an action of causing theswinging movement of the robot body, sooner or later, producing a slipabout the yaw axis between the foot sole of the robot and the floorsurface to affect the walking stability appreciably or otherwise provinga hindrance in the realization of the stable and accurate leggedoperations in an expected manner. If the factor of the moment about theyaw axis becomes excessive, robot falldown may be produced to destructthe robot body and an article against which the robot falls.

On the other hand, the legged mobile robot is made up by a plurality ofpart groups, such as left and right upper limbs, left and right lowerlimbs and a body trunk, and includes a large variety of degrees offreedom of the joints. It is however not known precisely how the pluralparts are to operate in concert in order to cancel out the moment errorsapplied to the robot body about the roll, pitch and yaw axes during itsmovements, such as walking.

In the posture stabilizing control for the robot, employing ZMP as acriterion for verifying the stability, a point with the zero pitch axismoment and the zero roll axis moment is searched within a supportpolygon defined by the floor touch point of the foot sole and the floorsurface. However, in case both leg units have cleared the floor surface,such as when the robot has jumped or descended from an elevated object,there is no support polygon, such that the conventional technique ofcontrolling the ZMP to be within the inside of the support polygoncannot be applied.

It is an object of the present invention to provide a method and anapparatus optimum for controlling the operation of the robot apparatus,and a computer program, in which the target trajectory for each part onthe robot body may be corrected to compensate an unknown external momentor an unknown external force.

It is another object of the present invention to provide a method and anapparatus optimum for controlling the operation of the robot apparatus,and a computer program, in which the moment error applied to the robotbody about the roll, pitch and yaw axes during the robot's movements,such as walking, may be canceled out with advantage by concertedoperations of the respective part groups making up the robot body.

It is another object of the present invention to provide a method and anapparatus optimum for controlling the operation of the robot apparatus,and a computer program, in which the moment error applied to the robotbody about the roll, pitch and yaw axes may be canceled out withadvantage even when the apparatus clears the floor surface, such asduring jumping or descending from an elevated place.

DISCLOSURE OF THE INVENTION

In view of the above depicted status of the art, the present inventionprovides, in its one aspect, a motion controlling apparatus and a motioncontrolling method for a robot apparatus having at least a plurality ofmovable parts, in which the motion controlling apparatus comprises ZMPequilibrium equation generating means for generating a ZMP equilibriumequation stating the relationship of equilibrium of moments applied to arobot body of the robot apparatus based on desirable motion data made upby trajectories of respective parts obtained, moment error calculatingmeans for calculating moment errors in the equilibrium equationgenerated by the ZMP equilibrium equation generating means, prioritysequence order setting means for setting the orders of the prioritysequence of the parts, the target trajectories of which are to becorrected for canceling out the moment error, and trajectory correctionmeans for correcting the target trajectories of the respective parts ina sequence prescribed by the so set priority sequence, and in which themotion controlling method comprises a ZMP equilibrium equationgenerating step of generating a ZMP equilibrium equation stating therelationship of equilibrium of moments applied to a robot body of therobot apparatus based on desirable motion data made up by trajectoriesof respective parts obtained, a moment error calculating step ofcalculating moment errors in the equilibrium equation generated by theZMP equilibrium equation generating step, a priority sequence ordersetting step of setting the orders of the priority sequence of the partsthe target (desirable) trajectories of which are to be corrected forcanceling out the moment error, and a trajectory correction step ofcorrecting the target (desirable) trajectories of the respective partsin a sequence prescribed by the so set priority sequence.

In particular, in a two-legged mobile robot, the moment about the yawaxis, applied to the robot body, increases appreciably with increase inthe walking speed, as pointed out above. This moment about the yaw axistends to cause turning, slip about the yaw axis relative to the floorsurface or falldown of the robot body, ascribable to loss of balance,and thus proves hindrance in realizing stable and accurate leggedoperations.

With the apparatus and the method for controlling the motion of therobot apparatus, according to the first aspect of the present invention,the moment errors, such as moment about the yaw axis, applied to therobot body on the so set ZMP in carrying out the movement pattern of therobot body, composed of the combination of the movements of the lowerlimbs, body trunk unit and the upper limbs, may be cancelled out bycorrecting the target (desirable) trajectories for the respective partsin accordance with a sequence in keeping with the preset prioritysequence.

Moreover, according to the present invention, the moment error appliedto the robot body about the roll, pitch and yaw axes during movements,such as walking, may be canceled out with advantage by concertedoperations of the respective part groups making up the robot body. Forexample, stability during legged operations, such as walking, may bemaintained by the movements of the upper limbs as the legged operationsby the lower limbs or by the body trunk are continued.

More specifically, the motion of the upper limbs is the motionexploiting the driving of the shoulder joints or the elbow joints.Although the left and right upper limbs naturally perform anti-phasemovements relative to each other, in general, this is not mandatory incarrying out the present invention.

In designing the general two-legged robot body, erected and walking onthe two legs, the mobile angle of the shoulder joint or the elbow jointis broader than that of the body trunk unit, such as the body trunk yawaxis. Thus, the yaw axis moment of the robot body may be efficiently andaccurately canceled out by such movements of the upper limbs, thusrealizing high posture stability.

Additionally, the upper limb movements make the motion of the upper halfof the robot body appear more expressive to exhibit what may be said tobe performance effects.

In the priority sequence order setting means or step, the prioritysequence order for correcting the target (desirable) trajectories may beaccorded in the order of the decreasing amount of the movable mass orthe decreasing amount of the generatable moment.

Or, in the priority sequence order setting means or step, the prioritysequence order for correcting or forming the desired trajectory may beaccorded to the respective parts by having reference to the pre-draftedaction schedule. For example, in case the robot apparatus is performinglegged motion, including the walking first of all, the trajectory of thelower limbs cannot be corrected, and hence a lower order of the prioritysequence is accorded to the lower limbs (in case of the prioritysequence of correcting the desired trajectory) or a higher order of thepriority sequence is accorded to the lower limbs (in case of thepriority sequence of not correcting the desired trajectory). Since thetrajectory of the hand grabbing an object cannot be corrected, a lowerorder of the priority sequence or a higher order of the prioritysequence is accorded to the hand. In case the robot apparatus is walkingsideways on a narrow alley, a lower order of the priority sequence or ahigher order of the priority sequence is accorded to the waist partbecause correction of the trajectory of the waist part tends to causecollision against the wall surface.

The ZMP equilibrium equation generating means or step may derive the ZMPequation for each of the pitch, roll and yaw axes. The trajectorycorrection means or step may correct the target (desirable) trajectoriesof the respective parts for each of the pitch, roll and yaw axes tocompensate the moment error.

Moreover, there is a problem that, if the robot has cleared the floor,as when the robot has jumped up or when the robot has descended from anelevated place, there is no support polygon, so that, in such case, theconventional technique of controlling the ZMP to be inside the supportpolygon cannot be applied. In the present invention, the equation ofmotion, rather than the ZMP equation, needs to be solved in case thereis no support acting point with respect to an exterior field or there isno ZMP within the support polygon, such as when the robot body hascleared the floor surface. For this reason, there may further beprovided center-of-gravity motion setting means or step for setting thecenter-of-gravity motion pertinent to the translation and/or rotation ofthe robot body when both legs of the legged mobile robot have clearedthe floor surface.

In such case, it is sufficient that the moment error calculating meanscalculates the amount of the force compensation at the target center ofgravity, in addition to the moment error in the ZMP equilibriumequation, the priority sequence order setting means sets the prioritysequence of the parts correcting the target (desirable) trajectory forcanceling the amounts of the moment error and the force compensation,and that the trajectory correction means corrects the targettrajectories for the respective parts in a sequence which is in keepingwith the so set priority sequence.

The present invention also provides, in another aspect, a computerprogram, stated in a computer-readable form, for allowing operationcontrol for a robot apparatus having at least a plurality of movableparts on a computer system, in which the program comprises a ZMPequilibrium equation generating step of generating a ZMP equilibriumequation stating the relationship of equilibrium of moments applied to arobot body of the robot apparatus based on desirable motion data made upby trajectories of respective parts obtained, a moment error calculatingstep of calculating moment errors in the equilibrium equation generatedby the ZMP equilibrium equation generating step, a priority sequenceorder setting step of setting the orders of the priority sequence of theparts, the target (desirable) trajectories of which are to be correctedfor canceling out the moment error, and a trajectory correction step ofcorrecting the target (desirable) trajectories of the respective partsin a sequence prescribed by the so set priority sequence.

The computer program, in the second aspect of the present invention, isa definition of a computer program stated in a computer readable formfor implementing a preset processing on a computer system. Stateddifferently, the computer program in the second aspect of the presentinvention may be installed on a computer system, whereby the cooperativeaction may be displayed on the computer system, such that the operationand effect similar to those of the apparatus and method for controllingthe motion of the robot apparatus according to the first aspect of thepresent invention may be achieved.

Other objects, features and advantages of the present invention willbecome more apparent from the following detailed description especiallywhen read in conjunction with the accompanying drawings.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1 shows the illustrative relationship between the walking speed ofthe two-legged walking robot (second/steps) and the moment producedalong the yaw axis direction (Nm).

FIG. 2 schematically shows the configuration of the degree-of-freedom ofa legged mobile robot 100 embodying the present invention.

FIG. 3 schematically shows the structure of the control system of thelegged-mobile robot 100.

FIG. 4 is a flowchart showing an example of an operational sequence forstabilizing processing of a whole-body exercise pattern of the robotbody.

FIG. 5 schematically shows an example of setting part groups of therobot body.

FIG. 6 shows the position of the center of gravity of a mass M7.

FIG. 7 is a flowchart showing another example of the operationalsequence for stabilizing processing of a whole-body exercise pattern ofthe robot body.

FIG. 8 is a flowchart showing a further example of the operationalsequence for stabilizing processing of a whole-body exercise pattern ofthe robot body.

BEST MODE FOR CARRYING OUT THE INVENTION

Referring to the drawings, preferred embodiments of the presentinvention will be explained in detail.

FIG. 2 schematically shows the configuration of the degree-of-freedom ofthe legged mobile robot 100 embodying the present invention.

The legged mobile robot 100, shown in FIG. 2, is a humanoid robot havingtwo legs and two arms. The present legged mobile robot 100 includes thefour limbs attached to a robot body, and is made up by left and rightarms, having seven degrees of freedom, namely a shoulder joint pitchaxis 101, a shoulder joint roll axis 102, an upper arm yaw axis 103, anelbow joint pitch axis 104, a forearm yaw axis 105, a wrist joint rollaxis 106 and a wrist joint pitch axis 107, and left and right legs,having six degrees of freedom, namely a hip joint yaw axis 108, a hipjoint roll axis 109, a hip joint pitch axis 110, a knee joint pitch axis111, an ankle joint pitch axis 112 and an ankle joint roll axis 113.

In actuality, these degrees of freedom of the respective joints areimplemented by actuator motors. In the present embodiment, a small-sizeddirect gear coupling AC servo motor of the type in which a servo controlsystem is arranged in one chip and enclosed in a motor unit.

Meanwhile, this sort of the AC servo actuator is disclosed in forexample the Japanese Laid-Open Patent Publication 2000-299970, alreadyassigned to the present Applicant (specification of the Japanese PatentApplication Number H1-33386).

The legged mobile robot 100 of the present embodiment uses the ZMP asthe criterion in verifying the walking stability. According to thecriterion of stability verification by ZMP, if a system delimits anappropriate ZMP space and the ZMP exists within a support polygon, norotational motion or translational motion occurs in the system, suchthat there is no necessity of solving an equation of motion pertainingto rotation or translation. However, it there is no ZMP within thesupport polygon or if there exists no support acting point for anexterior field, it is necessary to solve the equation of motion insteadof the ZMP equation, as will be explained subsequently.

FIG. 3 schematically shows a control system structure of the leggedmobile robot 100. As shown in the figure, the legged mobile robot 100 ismade up by mechanical units 30, 40, 50R/L and 60R/L, representing fourlimbs of the human being, and a control unit 80 for performing adaptivecontrol for realization of concerted movements among the respectivemechanical units (R and L are suffixes depicting right and left,respectively, hereinafter the same).

The movements of the legged mobile robot 100 in its entirety arecomprehensively controlled by the control unit 80. The control unit 80is made up by a main controller 81, composed of main circuit components,not shown, such as a CPU (central processing unit) and a memory, and aperipheral circuit 82, including an interface responsible for exchangingdata or commands with a power supply circuit or respective components ofthe robot 100.

In the realization of the present invention, there is no particularlimitation to the part of mounting the control unit 80. Although thecontrol unit is mounted on a body trunk unit 40, it may also be loadedon a head unit 30. Or, the control unit 80 may be mounted outside thelegged mobile robot 100 and communication may be made with the robotbody of the legged mobile robot 100 over a wired or wireless path.

The degrees of freedom of respective joints of the legged mobile robot100, shown in FIG. 2, are implemented by associated actuator motors M.That is, the head unit 30 is provided with a neck joint yaw axisactuator M₁, neck joint pitch axis actuators M_(2A), M_(2B), and a neckjoint roll axis actuator M₃, representing the neck joint yaw axis, firstand second neck joint pitch axes, and the neck joint roll axis,respectively.

The body trunk unit 40 includes a body trunk pitch axis actuator M₁₁ anda body trunk roll axis actuator M₁₂, representing the body trunk pitchaxis and the body trunk roll axis, respectively.

The arm units 50R/L, subdivided into upper arm units 51R/L, elbow jointunits 52R/L and forearm units 53R/L, are provided with a shoulder pitchaxis actuator M₄, a shoulder roll axis actuator M₅, an upper arm yawaxis actuator M₆, an elbow joint pitch axis actuator M₇, an elbow jointyaw axis actuator M₈, a wrist joint roll axis actuator M₉ and a wristjoint pitch axis actuator M₁₀, representing a shoulder joint pitch axis101, a shoulder joint roll axis 102, an upper arm yaw axis 103, an elbowjoint pitch axis 104, an elbow joint yaw axis 105, a wrist joint rollaxis 106 and a wrist joint pitch axis 107, respectively.

The leg unit 60R/L, sub-divided into a thigh unit 61R/L, a knee unit62R/L and a shank unit 63R/L, is provided with a hip joint yaw axisactuator M₁₃, a hip joint pitch axis actuator M₁₄, a hip joint roll axisactuator M₁₅, a knee joint pitch axis actuator M₁₆, an ankle joint pitchaxis actuator M₁₇ and an ankle joint roll axis actuator M₁₈,representing a hip joint yaw axis 108, a hip joint pitch axis 110, a hipjoint roll axis 109, a knee joint pitch axis 111, an ankle joint pitchaxis 112 and an ankle joint roll axis 113, respectively.

The actuators M₁, M₂, M₃, . . . , used for the respective joints, maypreferably be designed by actuator motors. In the present embodiment,each of these actuators may be the aforementioned small-sized directgear coupling type AC servo motor in which a servo control system isarranged in one chip and enclosed in a motor unit.

The head unit 30, body trunk unit 40, arm units 50R/L and the leg units60R/L are each provided with sub-controllers 35, 45, 55R/L and 65R/L fordriving controlling the respective actuators.

The body trunk unit 40 of the robot body is provided with a posturesensor G1, comprised e.g. of an acceleration sensor A1 and a gyrosensor. The acceleration sensor A1 is arranged e.g. in each of the X, Yand Z axes. By providing the acceleration sensor A1 to the waist unit ofthe robot body, it becomes possible to set the waist, as a part having alarge movable mass, as a control target point, to directly measure theposture as well as the acceleration at such location to manage ZMP-basedposture stabilizing control.

The leg units 60R/L are each provided with floor reaction force sensorsF1 to f4 and F5 to F8, acceleration sensors A2 and A3, and posturesensors G2 and G3. The floor reaction force sensors F1 to F8 are formedby mounting pressure sensors on the foot soles to detect whether or notthe foot sole has touched the floor based on the presence or absence ofthe force of reaction from the floor. The acceleration sensors A2 and A3are provided at least along the directions of the X and y axes. Bymounting the acceleration sensors A2 and A3 on the left and right footsoles, it is possible to construct the ZMP equilibrium equation directlyat the foot unit closest to the ZMP position.

If the acceleration sensor is provided solely in the waist unit as apart of a large movable mass, solely the waist unit is set as a controltarget point, so that the state of the foot unit has to be calculatedrelatively based on the results of calculations of the control targetpoint. Thus, the conditions:

-   (1) That the floor surface is not moved under any force or torque    applied thereto; and-   (2) that the frictional coefficient against translational motion on    the floor surface is sufficiently large such that no slip is    produced    need to be met as premises insofar as the foot unit and the floor    surface are concerned.

In contrast thereto, according to the present embodiment, a reactionforce sensor system, such as a floor reaction force sensor, for directlysensing the force and the ZMP, the local coordinate used for control andthe acceleration sensor for directly measuring the local coordinate, areprovided in a foot unit, as a part of contact of the robot body with thefloor surface. As a result, the ZMP equilibrium equation may be directlyformed at the foot unit closest to the ZMP position, so that more severeposture stabilizing control may be implemented with a high speed. Thus,stable walking (movements) of the robot body may be assured even on thegravel or on a thick-piled carpet where the ground surface is movedunder application of force or torque or on a tile of a house where asufficient frictional coefficient against translational motion cannot beprovided and hence the slip is likely to be produced.

The main controller (control unit 80) is able to dynamically correct thecontrol target responsive to outputs of the sensors A1 to A3, G1 to G3and F1 to F8. More specifically, the sub-controllers 35, 45, 55R/L and65R/L are adaptively controlled to realize the whole-body motion patternin which the upper limbs, body trunk and the lower limbs of the leggedmobile robot 100 are driven in concert.

In the whole-body motion of the robot body of the legged mobile robot100, the foot unit motion, ZMP trajectory, body trunk motion, upper limbmotion and the height of the waist unit, for example, are set, and thecommands specifying the movements in meeting with these setting contentsare transmitted to the sub-controllers 35, 45, 55R/L and 65R/L. Thesesub-controllers 35, 45, 55R/L and 65R/L interpret the commands receivedfrom the main controller 81 to output driving control signals to therespective actuators M₁, M₂, M₃, . . . The ‘ZMP’ herein means a point onthe floor surface where the moment by the reactive force form the floorduring walking of the robot is zero, and the ‘ZMP trajectory’ means atrajectory on which the ZMP is moved during the time period of walkingof the robot 100.

The sequence of operations for posture stabilization of the leggedmobile robot 100 of the present embodiment, during the operations onlegs, that is, during execution of the concerted whole body motionpattern of the foot units, waist, body trunk and the lower limbs, ishereinafter explained.

The legged mobile robot 100 of the present embodiment uses the ZMP asthe criterion for verifying the walking stability. According to thecriterion of stability verification by ZMP, if a system delimits anappropriate ZMP space and the ZMP exists within a support polygon, norotational motion or translation motion occurs in the system, such thatthere is no necessity of solving an equation of motion. However, itthere is no ZMP within the support polygon or if there exists no supportacting point for an exterior field, it is necessary to solve theequation of motion instead of the ZMP equation. For example, if therobot has cleared the floor, as when the robot has jumped up or when therobot has descended from an elevated place, there is no support polygon,so that, in such case, the equation of motion is solved in the presentembodiment in place of the ZMP equation, as later explained.

The ZMP equation of the robot body states the relationship ofequilibrium of the respective moments on the target ZMP. The ZMPequilibrium equation is such an equation which, in case a robot body isdepicted by plural mass points m_(i) as control target points, finds thetrajectory of each control point for which the sum of the moments on thetarget ZMP generated in the totality of the control target points m_(i)is zero.

The ZMP equilibrium equation (1) of the robot body, stated with theworld coordinate system (O-XYZ), and the ZMP equilibrium equation (2) ofthe robot body, stated with the local coordinate system of the robotbody (O-X′Y′Z′), follow:

$\begin{matrix}\left\{ \begin{matrix}{{{\sum\limits_{i}^{{all}\text{-}{particles}}\;{{m_{i}\left( {r_{i} - P_{zmp}} \right)} \times {\overset{¨}{r}}_{i}}} + T - {\sum\limits_{j}M_{j}} - {\sum\limits_{k}{\left( {S_{k} - P_{zmp}} \right) \times F_{k}}}} = 0} \\{r = {r^{\prime} + Q}}\end{matrix} \right. & (1) \\\left\{ \begin{matrix}{{\sum\limits_{i}^{{all}\text{-}{particles}}\;{{m_{i}\left( {{\overset{\_}{r}}_{i} - {\overset{\_}{P}}_{zmp}} \right)} \times \left( {{\overset{\overset{¨}{\_}}{r}}_{i} + \overset{\overset{¨}{\_}}{Q}} \right)}} + T - {\sum\limits_{j}M_{j}} -} \\{\mspace{464mu}{{\sum\limits_{k}{\left( {{\overset{\_}{S}}_{k} - {\overset{\_}{P}}_{zmp}} \right) \times F_{k}}} = 0}} \\{{\sum\limits_{i}^{{all}\text{-}{particles}}\;{m_{i}{\overset{\_}{r}}_{i} \times \left( {{\overset{\overset{¨}{\_}}{r}}_{i} + \overset{\overset{¨}{\_}}{Q}} \right)}} + T - {\sum\limits_{i}^{{all}\text{-}{particles}}\;{m_{i}{\overset{\_}{P}}_{zmp} \times \left( {{\overset{\overset{¨}{\_}}{r}}_{i} + \overset{\overset{¨}{\_}}{Q}} \right)}} + T -} \\{\mspace{374mu}{{{\sum\limits_{j}M_{j}} - {\sum\limits_{k}{\left( {{\overset{\_}{S}}_{k} - {\overset{\_}{P}}_{zmp}} \right) \times F_{k}}}} = 0}} \\{\overset{¨}{r} = {\overset{\overset{¨}{\_}}{r} + \overset{¨}{Q}}}\end{matrix} \right. & (2)\end{matrix}$

The above equations state that the sum of the moments about the ZMP(radius r_(i)−P_(zmp)) generated by the acceleration component appliedto each mass point (or control target point) m_(i), the sum of the j'thexternal force moments M_(j) applied and the sum of the moments aboutthe ZMP generated by the external force F_(k), with the point of actionof the k'th external force F_(k) being S_(k), are in equilibrium withone another.

The ZMP equilibrium equations include the floor reaction force moment Tat the target ZMP (moment error component). The posture stability of therobot body may be maintained by suppressing this moment error to zero orto within a preset allowable range. Stated differently, the essence ofthe posture stabilization control, with the ZMP as the criterion ofverifying the stability, is to correct the robot body motion, such asfoot unit motion, or the trajectories of the respective parts of theupper body portion, so that the moment error will be zero or not largerthan the allowable value.

In the present embodiment, since the acceleration sensors are providedon the foot soles of the foot units, as the contact part with the floorsurface, it is possible to set the local coordinate system of the actualrobot with respect to the world coordinate system, to find the foot soleof the foot unit as the point of origin and to directly derive the ZMPequilibrium equation. By providing the acceleration sensors to thecontrol target points, having large movable mass, including the waistunit first of all, the amounts of the moments about the ZMP for eachcontrol target point may be directly derived using the output values ofthe acceleration sensors.

Since the actual robot as the control target is a mobile system, it isdifficult to find the position vector on the world coordinate at eachcontrol target point. As an alternative method, the position vector ofthe control target point on the local coordinate system may be foundrather easily by inverse kinematic calculations. Hence, the latter ZMPequilibrium equation, stated with the local coordinate system of therobot body (O-X′Y′Z′), may be used to carry out the actual posturestabilization processing. It should be noted that, since it is difficultwith the state sensors loaded on the robot to correctly measure the ZMPtrajectory with the world coordinate system, an exterior measurementunit, fixed to the world coordinate system, is required in general, suchthat the action of the robot body by itself, such as walking, becomesimpossible. On the other hand, if a load sensor is provided on the floorcontact portion to measure the ZMP trajectory, it is possible to obtainthe ZMP trajectory with the local coordinate correctly and directly.Moreover, by providing the acceleration sensor in the vicinity of thepoint of origin of the local coordinate system, the information whichbecomes dominant in the high speed motion can be measured directly. Forthis reason, the ZMP equilibrium equation in the local coordinate systemis used in the present embodiment.

The posture stabilizing operations in the legged mobile robot 100 of thepresent embodiment, that is, the sequence of operations in generatingthe concerted whole-body motion pattern which cancels out the momentsgenerated by the movements of the waist unit, body trunk and the lowerlimbs, is hereinafter explained. The present embodiment is featured,first of all, by dividing the robot body of the legged mobile robot 100into plural parts, according the order in the priority sequence oftrajectory correction to each part, and by correcting the target(desired) trajectory from one part to another to cancel out the momenterror.

FIG. 4 depicts a flowchart showing an illustrative sequence ofoperations for stabilizing the whole-body motion pattern of the robotbody.

First, the movements of the floor contact parts, such as foot units, areset (step S1). The foot unit movements are motion data chronologicallyinterconnecting two or more poses of the robot body. The motion data aredata formulated and edited in advance, using e.g. a motion editingsystem, and is made up by the joint space information, representing thedisplacement of respective joint angles of the foot units, and theCartesian space information representing the joint positions.

The ZMP stable area is then calculated, based on the motion in the floorcontact part, as set, such as the foot unit (step S2). Or, the ZMPstable area is calculated, based on the desired trajectories, providedwith the orders of the priority sequence as set from one part group toanother (step S14 as later explained). Meanwhile, if the point ofcontact of the robot with the floor surface is not the foot sole, aswhen the robot performs floor exercise or handstand, the ZMP stable areais calculated, based on the support polygon as defined by the floorcontact point other than the foot sole and the floor surface.

The ZMP is a point about which the moments applied to the robot body areequal to zero, as discussed above. Basically, the ZMP is inside asupport polygon defined by the floor contact points of the foot solesand the floor surface. The ZMP stable area is set inside this supportpolygon. The robot body may be in a highly stabilized state by havingthe ZMP accommodated in this ZMP stable area.

The ZMP trajectory during the foot unit motion is set, based on the footunit motion and the ZMP stable area (step S3). Meanwhile, if the pointof contact of the robot with the floor surface is not the foot sole, aswhen the robot performs floor exercise or handstand, the ZMP trajectoryis calculated, based on the support polygon as defined by the floorcontact point other than the foot sole and the floor surface.

The respective parts of the robot body are grouped, such as waist unit,body trunk, upper limbs and head unit (step S11).

FIG. 5 shows an example of setting of the part groups of the robot body.In the example shown in FIG. 5, first to seventh part groups, namely awaist unit, a body trunk, a right arm unit, a head unit, a left armunit, a left leg unit and a right leg unit, are set. Each part group ismade up by plural mass points. These mass points may be set manually byan implementer or automatically in accordance with a preset rule (forexample, locations with large movable mass are set as mass points). Ifthe number of the mass points of an i'th part is n_(i), the total weightmass m_(i) of the i'th part group (i'th unit) is given by the followingequation (3):

$\begin{matrix}{m_{i} = {\sum\limits_{i}^{n_{i}}m_{i}}} & (3)\end{matrix}$

A desired trajectory is set from one part group to another (step S12).The desired trajectory for each part group of the robot body is setoff-line, with the aid of, for example, a motion editing system.

The group setting of each part is then adjusted by way of re-grouping(step S13). The order of the priority sequence is accorded to each ofthe part groups (step S14). The order of the priority sequence heremeans the order used in processing calculations for posture stabilizingcontrol of the robot body and is determined depending on amount of themovable mass or amount of the generatable moment. The result is that aset of desired trajectories, provided with the order of the prioritysequence, are completed for the respective parts of the robot body. Atthis time, the conditions for connection of the respective parts, suchas local coordinate connection, position-fixed connection to the worldcoordinate of the specified part or the world coordinate postureconnection of the specified parts, are also set. The priority sequencemay also be varied at time t.

The order in the priority sequence for each part is determined byreferencing a pre-drafted action schedule, in addition to amount of themovable mass or amount of the generatable moment. For example, when therobot is performing legged movements, including the walking, first ofall, the lower order in the priority sequence is accorded to the footunit, contacting the floor, because the trajectory of the foot unitcontacting the floor cannot be corrected. The trajectory of a hand forgrabbing an article cannot be corrected, and hence a lower order in thepriority sequence is accorded to the hand. When the robot is walking ona narrow alley sideways, a lower order in the priority sequence isaccorded to the waist unit, because the waist unit is likely to impingeon a wall if the trajectory of the waist unit is changed.

Moreover, for each part group of the robot body, the weight mass thatmay be used for moment compensation is calculated (step S15). In aninstance of FIG. 5, since the right leg is a support leg and the ZMPposition P_(ZMP) is present on the right leg, the weight mass that maybe used for moment compensation in each part group is as shown in thefollowing equation (4):

$\begin{matrix}\left\{ \begin{matrix}{M_{1} = {\sum\limits_{m = 1}^{5}m_{m}}} \\{M_{2} = {\sum\limits_{m = 2}^{5}m_{m}}} \\{M_{3} = m_{3}} \\{M_{4} = m_{4}} \\{M_{5} = m_{5}} \\{M_{6} = m_{6}} \\{M_{7} = {\sum\limits_{m = 1}^{7}m_{m}}}\end{matrix} \right. & (4)\end{matrix}$

With the first unit, that is the waist unit, the body trunk unit, rightarm unit, head unit and the left arm unit, supported by the waist unit,may be used, in addition to the waist unit itself, for momentcompensation. With the second unit, that is, the body trunk unit, theright arm unit, head unit and the left arm unit, supported by the bodytrunk unit, may be used, in addition to the body trunk unit itself, formoment compensation. With the third to sixth units, that is, the rightarm unit, head unit, left arm unit and the left leg unit, as a foot unitin flight, there are no part groups supported by these units, so thatonly the own units represent the weight masses that may be used formoment compensation. With the seventh unit, that is, the right leg unit,as a leg in stance, supporting the totality of the part groups, the sumM₇ of the weight masses of all parts of the robot body may be used formoment compensation. The center of gravity position of the weight massM₇ in this case corresponds to the center of gravity position of therobot body, rather than to that of the leg unit (see FIG. 6).

Based on the motion of the floor contact parts, such as the foot unit,the ZMP trajectory and the set of the desired trajectories of each ofthe part groups of the robot body, the motion patterns of the respectivepart groups are put into the posture stabilizing processing.

In this posture stabilizing processing, an initial value of 1 issubstituted for a processing variable i (step S20). Then, the amount ofthe moment on the target ZMP, that is, the total amount of the momentcompensation Ω in case a target trajectory is set in each part group, iscalculated (step S21), as shown in the following equation (5):

$\begin{matrix}{\Omega = {{\sum\limits_{i}^{all\_ particles}{{m_{i}\left( {r_{i} - P_{zmp}} \right)} \times {\overset{¨}{r}}_{i}}} - {\sum\limits_{j}M_{j}} - {\sum\limits_{k}{\left( {S_{k} - P_{zmp}} \right) \times F_{k}}}}} & (5)\end{matrix}$

It should be noted that the total amount of the moment compensation Ω isthe sum of the amounts of the moment on the target ZMP of each partgroup plus the moment of the unknown external force and the unknownexternal force, defined in the above equations, as known terms.

For the part where the target trajectory has not been calculated, adesired trajectory is used.

Then, using the weight mass M_(i), that may be used for momentcompensation of the part i in question, calculated in the step S15, anabsolute moment compensation coefficient α_(i) thereof is set (step S22)and the amount of the moment compensation Ω_(i) in the part group i iscalculated (step S23), as shown in the following equation (6):

$\begin{matrix}\left\{ \begin{matrix}{\Omega_{i} = {\frac{\alpha_{i}}{\sum\limits_{i}^{n}\alpha_{i}}\Omega}} \\{{\alpha_{i} = {\beta_{i} \times \frac{M_{i}}{\sum\limits_{i = 1}^{n}M_{i}}}},} \\{{{in}\mspace{14mu}{which}},{{\sum\limits_{i = 1}^{n}\alpha_{i}} = 1.0},} \\{\beta_{i}\text{:}\mspace{11mu}{amount}\mspace{14mu}{of}\mspace{14mu}{relative}\mspace{14mu}{moment}\mspace{14mu}{{compensation}.}}\end{matrix} \right. & (6)\end{matrix}$

Then, using the amount of the moment compensation Ω_(i) of the i'thpart, as calculated, the ZMP equation pertinent to the i'th part isderived (step S24), as indicated by the following equation (7):

$\begin{matrix}\begin{matrix}{{{sum}\mspace{14mu}{of}\mspace{14mu}{mass}\mspace{14mu}{points}\mspace{14mu}{that}\mspace{14mu}{may}\mspace{14mu}{be}\mspace{14mu}{used}\mspace{14mu}{by}\mspace{14mu}{the}\mspace{14mu} i^{\prime}{th}}\mspace{14mu}} \\{{{site}\mspace{14mu}{for}\mspace{14mu}{moment}\mspace{14mu}{compensation}}\mspace{14mu}} \\{{{\sum\limits_{i}{{m_{i}\left( {{\Delta\; r_{i}} - P_{zmp}} \right)} \times \Delta\;{\overset{¨}{r}}_{i}}} + \Omega_{i}} = 0}\end{matrix} & (7)\end{matrix}$

However, with the above ZMP equation, containing many unknown variables,it is difficult to find a solution by analytical routes or by numericalcalculations. Thus, an approximate ZMP equation, indicated by thefollowing equation (8):M _(i)(Δr _(M) _(i) −P _(zmp))×Δ{umlaut over (r)} _(M) _(i)+Ω_(i)=0  (8)is here derived, in which the target center-of-gravity trajectory r_(Mi)of the weight mass usable for the moment compensation of the i'th part,is a known variable, and in which an amount of change Δr_(Mi) to thetarget center-of-gravity trajectory of the weight mass usable for momentcompensation of the i'th part is an unknown variable. In the aboveequation, r_(Mi) is the position vector of the center of gravity of theweight mass M_(i) usable for moment compensation in the i'th part group.

This approximate ZMP equation is first solved to calculate the amount ofchange Δr_(Mi) with respect to the target center-of-gravity trajectoryof the weight mass usable for moment compensation of the i'th part. Thetarget center-of-gravity trajectory of the weight mass usable for momentcompensation of the i'th part is calculated by the following equation(9):

$\begin{matrix}{r_{i_{M_{i}}} = {{\Delta\; r_{M_{i}}} + {r_{M_{i}}.}}} & (9)\end{matrix}$

In the case of the legged mobile robot, the links of which areinterconnected by rotational joints, the equation (8) in general co-ownsmovements in the Z-direction and hence becomes a non-linear two orderdifferential equation presenting the interference and which is difficultto solve analytically. Thus, the following equation (10):

$\begin{matrix}{{r_{i_{M_{iz}}} = {{constant}\mspace{14mu}{value}}},{{\Delta\;{\overset{¨}{r}}_{M_{iz}}} = 0}} & (10)\end{matrix}$not co-owning the movements in the Z-direction is used to render theequation linear and free of interference.

An approximate solution of the target center-of-gravity trajectory ofthe weight mass usable for moment compensation of the i'th part iscalculated, an amount of change Δr_(i) of each mass point r_(i) of thei'th part, that may be calculated from this approximate solution, issubstituted into the equation (7) as the strict ZMP equation to find amoment error. This error with the sign inverted is accumulated in theright side of the equation (8) rendered linear and free of interference,and an approximate solution is again found. This sequence of operationsis repeated until the error is not larger than a preset allowable valueto calculate the moment compensating motion of the part in question(step S25). In this manner, the target trajectory for each of the partshaving the first to i'th order in the priority sequence.

For calculating the trajectories of the respective mass points of thei'th part from the approximate solution of the target center-of-gravitytrajectory of the weight mass usable for moment compensation of the i'thpart, an operating point is placed in an i'th optional part, and acenter-of-gravity vector C (X, Y, Z, θ_(x), θ_(y), θ_(z)), having thetranslational positions (X, Y, Z) or the rotational angles (θ_(x),θ_(y), θ_(z)) as unknown variable, is derived. An equation C (X, Y, Z,θ_(x), θ_(y), θ_(z))=r_(iMi), in which the center-of-gravity vector C(X, Y, Z, θ_(x, θ) _(y), θ_(z)) is arranged as an unknown variable onthe left side and in which the center-of-gravity position vector r_(Mi)of the weight mass M_(i) is arranged as a known variable on the rightside, is derived, and the solution of this equation is calculated by anumerical value search method or found analytically.

This processing is carried out for all part groups to generate awhole-body motion pattern capable of performing stable movements, e.g.walking.

With the processing sequence, shown in FIG. 4, the robot body of thelegged mobile robot 100 is imaginarily divided into plural parts, theorder of the priority sequence for trajectory correction is accorded toeach part and the target (desired) trajectory is corrected from one partto another to cancel out the moment error in the whole-body concertedmotion. It should be noted that the posture stabilizing processingemploying the ZMP as the criterion for verifying the stability basicallyresides in searching a point of zero pitch axis moment and zero rollaxis moment within a support polygon defined by the floor contact point,such as the foot sole, and the floor surface. However, there is aproblem that the moment error about the yaw axis, that is, about theZ-axis, of the robot body is not explicitly coped with.

In a modification of the above-described embodiment, the moment errorsapplied to the robot body about the roll, pitch and yaw axes in thewhole body concerted movement, such as walking, are canceled out. FIG. 7shows, in a flowchart, the processing sequence for canceling out themoment errors about the roll, pitch and yaw axes of the robot body tostabilize the whole-body motion pattern.

First, the motion of the foot unit is set (step S31). The motion of thefoot unit is motion data made up by chronologically interconnected twoor more poses of the robot body. The motion data is made up by the jointspace information representing the displacement of the respective jointangles of the foot unit and the Cartesian space information representingthe joint positions (same step).

The ZMP stable area is then calculated, based on the movements in thefloor contact parts, as set, such as the foot unit (step S32). Or, theZMP stable area is calculated, based on the desired trajectory, providedwith the priority sequence, as set from one part group to another (steps44 as later explained). Meanwhile, if the contact point of the robotwith the floor surface is not the foot unit, as when the robot isperforming floor exercise or a handstand, the ZMP stable area iscalculated based on the support polygon defined by the floor contactpoint other than the foot unit and the floor surface.

The ZMP is a point where the moment applied to the robot body becomeszero, as discussed previously. Basically, the ZMP is in the inside of asupport polygon defined by the floor contact point of the foot sole andthe floor surface. The ZMP stable area is set inwardly of this supportpolygon. With the ZMP in this area, the robot body can be maintained ina highly stabilized state.

Based on the motion of the foot unit and the ZMP stable area, the ZMPtrajectory during the motion of the foot unit is set (step S33).However, if the contact point of the robot with the floor surface is notthe foot unit, as when the robot is performing floor exercise or ahandstand, the ZMP trajectory is set based on the support polygondefined by the floor contact point other than the foot unit and thefloor surface.

The respective parts of the robot body are set into groups, such as intoa waist unit, a body trunk unit, upper limbs and a head unit. Aninstance of part group setting of the robot body is shown in FIG. 5,where the first to seventh part groups are set in the sequence of thewaist unit, body trunk unit, right arm unit, head unit, left arm unit,left leg unit and the right leg unit.

A desired trajectory is set for each part group (step S42). The desiredtrajectory for each part group of the robot body is set off-line, usinge.g. a motion editing system.

The group setting of the respective parts is adjusted, by way ofregrouping (step S43) and the order of the priority sequence is accordedto each group (step S44). The priority sequence is the order by whichthe parts carrying the order are put into the processing calculationsfor posture stabilizing control of the robot body, and is accorded independence upon the amount of the movable mass or the amount of thegeneratable moment. As a result, a set of desired trajectories providedwith the orders of the priority sequence is formed for each part of therobot body. The order of the priority sequence is ρ and is made up bythree elements, namely a pitch axis priority sequence ρ_(p), roll axispriority sequence ρ_(r) and a yaw axis priority sequence ρ_(yaw).Moreover, the priority sequence of each part is determined byreferencing a pre-drafted action schedule, in addition to amount of themovable mass or amount of the generatable moment. At this time, theconditions for connection of the respective parts, such as localcoordinate connection, position-fixed connection to the world coordinateof the specified part or the posture connection to the world coordinateof the specified parts, are also set. The priority sequence may also bevaried at time t.

The weight mass usable for moment compensation is calculated from onepart group of the robot body to another (step S45). In the instanceshown in FIG. 5, since the right leg is the leg in stance and the ZMPposition P_(ZMP) is on the right leg unit, the weight mass Mi usable formoment compensation in each part group is as described above.

Based on the motion of the floor contact parts, such as the foot units,the ZMP trajectory and the set of the desired trajectories of each ofthe part groups of the robot body, the motion patterns of the respectivepart groups are put into the posture stabilizing processing, inaccordance with the priority sequence as set in the step S44.

In this posture stabilizing processing, an initial value of 1 issubstituted in a processing variable i (step S50). Then, the amount ofthe moment on the target ZMP, that is, the total amount of the momentcompensation Ω in case a target trajectory is set in each of thetotality of the part groups, is calculated (step S51), as shown in thefollowing equation (11):

$\begin{matrix}{\Omega = {{\sum\limits_{i}^{all\_ particles}{{m_{i}\left( {r_{i} - P_{zmp}} \right)} \times {\overset{¨}{r}}_{i}}} - {\sum\limits_{j}M_{j}} - {\sum\limits_{k}{\left( {S_{k} - P_{zmp}} \right) \times {F_{k}.}}}}} & (11)\end{matrix}$

It should be noted that the total amount of the moment compensation Ω isthe sum of the amounts of the moment on the target ZMP of the totalityof the part groups plus the moment of the unknown external force and theunknown external force, defined in the above equations, as known terms.

For the part where the target trajectory has not been calculated, adesired trajectory is used.

Then, using the weight mass M_(i), that may be used for momentcompensation of the part i in question, calculated in the step S45, anabsolute moment compensation coefficient α_(i) thereof is set (step S52)and the amount of the moment compensation Ω_(i) in the part group i iscalculated (step S53), as shown in the following equation (12):

$\begin{matrix}\left\{ \begin{matrix}{\Omega_{i} = {\frac{\alpha_{i}}{\sum\limits_{i}^{n}\alpha_{i}}\Omega}} \\{{\alpha_{i} = {\beta_{i} \times \frac{M_{i}}{\sum\limits_{i = 1}^{n}M_{i}}}},} \\{{{in}\mspace{14mu}{which}},{{\sum\limits_{i = 1}^{n}\alpha_{i}} = 1.0},} \\{\beta_{i}\text{:}\mspace{11mu}{amount}\mspace{14mu}{of}\mspace{14mu}{relative}\mspace{14mu}{moment}\mspace{14mu}{compensation}}\end{matrix} \right. & (12)\end{matrix}$

Then, using the amount of the moment compensation Ω_(i) of the i'thpart, as calculated, the ZMP equation pertinent to the i'th part isderived (step S54), as indicated by the following equation (13):

$\begin{matrix}\begin{matrix}{{{sum}\mspace{14mu}{of}\mspace{14mu}{mass}\mspace{14mu}{points}\mspace{14mu}{that}\mspace{14mu}{may}\mspace{14mu}{be}\mspace{14mu}{used}\mspace{14mu}{by}\mspace{14mu}{the}\mspace{14mu} i^{\prime}{th}}\mspace{14mu}} \\{{{site}\mspace{14mu}{for}\mspace{14mu}{moment}\mspace{14mu}{compensation}}\mspace{14mu}} \\{{{\sum\limits_{i}{{m_{i}\left( {{\Delta\; r_{i}} - P_{zmp}} \right)} \times \Delta\;{\overset{¨}{r}}_{i}}} + \Omega_{i}} = 0}\end{matrix} & (13)\end{matrix}$

However, with the above ZMP equation, containing many unknown variables,it is difficult to find a solution by analytical routes or by numericalcalculations. Thus, an approximate ZMP equation, indicated by thefollowing equation (14):M _(i)(Δr _(M) _(i) −P _(zmp))×Δ{umlaut over (r)} _(M) _(i)+Ω_(i)=0  (14)is here derived, in which the target center-of-gravity trajectory r_(Mi)of the weight mass usable for the moment compensation of the i'th part,is a known variable, and in which an amount of change Δr_(Mi) to thetarget center-of-gravity trajectory of the weight mass usable for momentcompensation of the i'th part is an unknown variable. In the aboveequation, r_(Mi) is the position vector of the center of gravity of theweight mass M_(i) usable for moment compensation in the i'th part group.

This approximate ZMP equation is first solved to calculate the amount ofchange Δr_(Mi) of the weight mass usable for moment compensation of thei'th part with respect to the target center-of-gravity trajectory. Thetarget center-of-gravity trajectory of the weight mass usable for momentcompensation of the i'th part is calculated by the following equation(15):

$\begin{matrix}{r_{i_{M_{i}}} = {{\Delta\; r_{M_{i}}} + r_{M_{i}}}} & (15)\end{matrix}$

In the case of the legged mobile robot, the links of which areinterconnected by rotational joints, the equation (14) in generalco-owns movements in the Z-direction and hence becomes a non-linear twoorder differential equation presenting the interference and which isdifficult to solve analytically. Thus, the following equation (16):

$\begin{matrix}{{r_{i_{M_{iz}}} = {{constant}\mspace{14mu}{value}}},{{\Delta\;{\overset{¨}{r}}_{M_{iz}}} = 0}} & (16)\end{matrix}$not co-owning the motion in the Z-direction is used to render theequation linear and free of interference.

An approximate solution of the target center-of-gravity trajectory ofthe weight mass usable for moment compensation of the i'th part iscalculated, an amount of change Δr_(i) of each mass point r_(i) of thei'th part, that may be calculated from this approximate solution, issubstituted in the equation (13), as the strict ZMP, to find a momenterror. This error with the sign inverted is accumulated in the rightside of the equation (14), rendered linear and free of interference, andan approximate solution is again found. This sequence of operations isrepeated until the error is not larger than a preset allowable value tocalculate the moment compensating movement of the part in question (stepS55). In this manner, the target trajectory may be obtained for each ofthe parts having the first to i'th orders in the priority sequence.

For calculating the trajectories of the respective mass points of thei'th part from the approximate solution of the target center-of-gravitytrajectory of the weight mass usable for moment compensation of the i'thpart, an operating point is placed in an i'th optional part, and acenter-of-gravity vector C (X, Y, Z, θ_(x), θ_(y), θ_(z)), having thetranslational positions (X, Y, Z) or the rotational angles (θ_(x),θ_(y), θ_(z)) as unknown variables, is derived. An equation C (X, Y, Z,θ_(x), θ_(y), θ_(z))=r_(iMi), in which the center-of-gravity vector C(X, Y, Z, θ_(x), θ_(y), θ_(z)) is arranged as an unknown variable on theleft side and in which the center-of-gravity position vector r_(Mi) ofthe weight mass M_(i) is arranged as a known variable on the right side,is derived, and the solution of this equation is calculated by anumerical value search method or found analytically.

There are occasions where the movements satisfying the priority sequenceρ (ρ_(p), ρ_(r), ρ_(yaw)) do not exist, depending on the configurationof the degree of freedom of the i'th part, as when the interference withrespect to the pitch or roll motion is small or when there is no degreeof freedom of the yaw-axis motion. In such case, the processing revertsto the step S44 to re-set the priority sequence.

This processing is carried out for all part groups to generate awhole-body motion pattern capable of performing stable movements, e.g.walking.

With the processing sequence, shown in FIG. 7, the robot body of thelegged mobile robot 100 is imaginarily divided into plural parts, theorder of the priority sequence for trajectory correction is accorded toeach part and the whole-body motion pattern may be stabilized in such amanner as to cancel out the moment error applied to the robot body aboutthe roll, pitch and yaw axes. The posture stabilizing control of therobot, employing the ZMP as the criterion for verifying the stability,searches a point of the zero pitch axis moment and the zero roll axismoment in the inside of the support polygon defined by the floor contactpoint of the foot sole and the floor surface. However, there is aproblem that, if the robot has cleared the floor, as when the robot hasjumped up or when the robot has descended from an elevated location,there is no support polygon, so that, in such case, the conventionaltechnique of controlling the ZMP to be inside the support polygon cannotbe applied.

Thus, as a modification of the present embodiment, it is assumed thatthere is no ZMP within the support polygon, as when the foot unit of therobot clears the floor, or when there is no support point for theoutside. FIG. 8 shows, as a flowchart, a further sequence of operationsfor stabilizing processing of the whole-body motion pattern of the robotbody. In the processing sequence, shown in FIG. 8, it is assumed thatthere is no ZMP within the support polygon, as when the foot unit of therobot has cleared the floor or when there is no support point for theoutside field, and an equation of motion is solved in addition to theZMP equation.

First, the motion of the foot unit is set (step S61). The foot unitmovements are motion data chronologically interconnecting two or moreposes of the robot body. The motion data is made up by the joint spaceinformation, representing the displacement of respective joint angles ofthe foot units, and the Cartesian space information representing thejoint positions (step S61).

The ZMP stable area is then calculated, based on the motion in the floorcontact part, as set, such as the foot unit (step S62). Or, the ZMPstable area is calculated, based on the desired trajectory, providedwith the order of the priority sequence so set from one part group toanother (step S74 as later explained). Meanwhile, if the point ofcontact of the robot with the floor surface is not the foot sole, aswhen the robot performs floor exercise or handstand, the ZMP stable areais calculated, based on the support polygon as defined by the floorcontact point other than the foot sole and the floor surface.

The ZMP is a point about which the moments applied to the robot body areequal to zero, as discussed above. Basically, the ZMP is inside asupport polygon defined by the floor contact points of the foot solesand the floor surface. The ZMP stable area is set further inside thissupport polygon. The robot body may be in a highly stabilized state byhaving the ZMP accommodated in this ZMP stable area.

The ZMP trajectory during the foot unit motion is set, based on the footunit motion and the ZMP stable area (step S63). Meanwhile, if the pointof contact of the robot with the floor surface is not the foot sole, aswhen the robot performs floor exercise or handstand, the ZMP trajectoryis set, based on the support polygon as defined by the floor contactpoint other than the foot sole and the floor surface.

Moreover, it is assumed that there is no ZMP within the support polygon,as when the foot unit of the robot has cleared the floor or that thereis no support point for the outside, and an equation of motion is solvedin addition to the ZMP equation. Hence, the center-of-gravity motion(translation and rotation) is set (step S91).

Moreover, it is assumed that there is no ZMP within the support polygon,as when the foot unit of the robot has cleared the floor or that thereis no support point for the outside, and an equation of motion is solvedin addition to the ZMP equation. Hence, the center-of-gravity motion(translation and rotation) is set (step S91).

If desired to suppress posture changes in the robot body at an instantthe robot has touched the floor after jumping or after descent from anelevated place, it is sufficient if, directly after the step S91, theimaginary ZMP is calculated and a desired trajectory of the motionpertinent to the part of touchdown is set so that the imaginary ZMP willbe in the support polygon formed on touchdown. The ‘imaginary ZMP’ isthe ZMP that can be found as a result of calculations of the ZMP underthe assumption that the support polygon actually cannot be formed but alarge support polygon is formed on the estimated touchdown floor surfaceregion.

The respective parts of the robot body are grouped, such as waist unit,body trunk, upper limbs and head unit (step S71). As an example ofsetting of the part groups, shown in FIG. 5, first to seventh partgroups, namely a waist unit, a body trunk, a right arm unit, a headunit, a left arm unit, a left leg unit and a right leg unit, are set.

A desired trajectory is set from one part group to another (step S72).The desired trajectory for each part group of the robot body is setoff-line, with the aid of, for example, a motion editing system.

The group setting of each part is then adjusted by way of re-grouping(step S73). The order of the priority sequence is accorded to each ofthe part groups (step S74). The order of the priority sequence heremeans the order used in the processing calculations for performingposture stabilizing control, and is determined depending on amount ofthe movable mass or amount of the generatable moment. The result is thata set of desired trajectories, provided with the order of the prioritysequence, are completed for each of the respective parts of the robotbody. The priority sequence ρ is formed by six elements, namely a pitchaxis priority sequence ρ_(p), a roll axis priority sequence ρ_(r), a yawaxis priority sequence ρ_(yaw), an X-axis priority sequence ρ_(x), aY-axis priority sequence ρ_(y) and a Z-axis priority sequence ρ_(z). Theorder in the priority sequence for each part is determined byreferencing a pre-drafted action schedule, in addition to amount of themovable mass or amount of the generatable moment. At this time, theconditions for connection of the respective parts, such as localcoordinate connection, position-fixed connection to the world coordinateof the specified part or posture connection to the world coordinate ofthe specified parts, are also set. The priority sequence may also bevaried at time t.

For each part group of the robot body, the weight mass usable for momentcompensation and force compensation is calculated (step S75). In theinstance shown in FIG. 5, in which the right leg is the leg in stanceand the ZMP position P_(ZMP) is on the right leg unit, the weight massM_(i) usable for moment compensation in each part group is as describedabove.

Based on the motion of the floor contact parts, such as the foot unit,the ZMP trajectory and the set of the desired trajectories of each ofthe part groups of the robot body, the motion patterns of the respectivepart groups are put into the posture stabilizing processing.

In this posture stabilizing processing, an initial value of 1 issubstituted in a processing variable i (step S80). The amount of themoment on the target ZMP, that is, the amount of total momentcompensation Ω and the amount of force compensation Γ in case the targettrajectory has been set on each of the part groups, are calculated (stepS81), as shown in the equations (17), (18):

$\begin{matrix}{\Omega = {{\sum\limits_{i}^{all\_ particles}{{m_{i}\left( {r_{i} - P_{zmp}} \right)} \times {\overset{¨}{r}}_{i}}} - {\sum\limits_{j}M_{j}} - {\sum\limits_{k}{\left( {S_{k} - P_{zmp}} \right) \times F_{k}}}}} & (17) \\\left\{ \begin{matrix}{\Gamma = {{\sum\limits_{i}^{all\_ particles}{m_{i}{\overset{¨}{r}}_{i}}} - {M_{c}{\overset{¨}{r}}_{c}} - {\sum\limits_{j}{M_{j}/\left( {S_{k} - P_{zmp}} \right)}} - {\sum\limits_{k}F_{k}}}} \\{M_{c}\text{:}\mspace{11mu}{total}\mspace{14mu}{weight}\mspace{14mu}{mass}} \\{{\overset{¨}{r}}_{c}\text{:}\mspace{11mu}{target}\mspace{14mu}{acceleration}\mspace{14mu}{of}\mspace{14mu}{center}\mspace{14mu}{of}\mspace{14mu}{gravity}}\end{matrix} \right. & (18)\end{matrix}$

It is noted that the amount of total moment compensation Ω is the sum ofthe amount of moment on the target ZMP of the totality of the partgroups plus the moment of the unknown external force and the unknownexternal force, defined in the above equations, as known terms, and thatthe amount of force compensation Γ is the sum of the amount of forcecompensation on the target ZMP of the totality of the part groups plusthe moment of the unknown external force and the unknown external force,defined in the above equations, as known terms.

For the part where the target trajectory is not calculated, a desiredtrajectory is used.

Then, using the weight mass M_(i), calculated by the step S75, and whichis usable for moment compensation and force compensation of the part iin question, an absolute moment compensation amount coefficient α_(i)and an absolute force compensation amount coefficient χ_(i) are set(step S82), and the amount of moment compensation Ω_(i) and the amountof force compensation σ_(i) are calculated in accordance with thefollowing equation (19) (step S83):

$\begin{matrix}\left\{ \begin{matrix}{\Omega_{i} = \frac{\alpha_{i}}{\sum\limits_{i}^{n}\alpha_{i}}} \\{\Gamma_{i} = {\frac{\chi_{i}}{i{\sum\limits_{i}^{n}\chi_{i}}}\Gamma}} \\\begin{matrix}{{\alpha_{i} = {\beta_{i} \times \frac{M_{i}}{\sum\limits_{i = 1}^{n}M_{i}}}},{{in}\mspace{14mu}{which}},{{\sum\limits_{i = 1}^{n}\alpha_{i}} = 1.0},{\beta_{i}:}} \\{{amount}\mspace{14mu}{of}\mspace{14mu}{relative}\mspace{14mu}{moment}\mspace{14mu}{compensation}}\end{matrix} \\\begin{matrix}{{\chi_{i} = {\delta_{i} \times \frac{M_{i}}{\sum\limits_{i = 1}^{n}M_{i}}}},{{in}\mspace{14mu}{which}},{{\sum\limits_{i = 1}^{n}\chi_{i}} = 1.0},{\delta_{i}:}} \\{{amount}\mspace{14mu}{of}\mspace{14mu}{relative}\mspace{14mu}{force}\mspace{14mu}{compensation}}\end{matrix}\end{matrix} \right. & (19)\end{matrix}$

Then, using the amount of moment compensation Ω_(i) and the amount offorce compensation Γ_(i) of the i'th part, as calculated, the ZMPequation for the i'th part, shown in the following equation (20), theequation of motion pertinent to rotation at the center of gravity, shownin the following equation (24) to follow, and the equation of motionpertinent to translation, shown in the following equation (21), arederived (step S84).

$\begin{matrix}{{{total}\mspace{14mu}{mass}\mspace{14mu}{points}\mspace{14mu}{usable}\mspace{14mu}{by}\mspace{14mu}{the}\mspace{14mu} i^{\prime}{th}\mspace{14mu}{site}\mspace{14mu}{for}\mspace{14mu}{moment}\mspace{14mu}{compensation}}\text{}{{{\sum\limits_{i}{{m_{i}\left( {{\Delta\; r_{i}} - P_{zmp}} \right)} \times \Delta\;{\overset{¨}{r}}_{i}}} + \Omega_{i}} = 0}} & (20) \\{{{total}\mspace{14mu}{mass}\mspace{14mu}{points}\mspace{14mu}{usable}\mspace{14mu}{by}\mspace{14mu}{the}\mspace{14mu} i^{\prime}{th}\mspace{14mu}{site}\mspace{14mu}{for}\mspace{14mu}{force}\mspace{11mu}{compensation}}{{{\sum\limits_{i}{m_{i}\Delta\;{\overset{¨}{r}}_{i}}} + \Gamma_{i}} = 0}} & (21)\end{matrix}$

However, with the above ZMP equation, containing many unknown variables,it is difficult to find a solution by analytical routes or by numericalcalculations. Thus, an approximate ZMP equation, indicated by thefollowing equation (22), or an approximate equation of motion, pertinentto rotation at the center of gravity, indicated by the followingequation (25), in which the target center-of-gravity trajectory r_(Mi)of the weight mass usable for the moment compensation and forcecompensation of the i'th part, respectively, are known variables, and inwhich an amount of change Δr_(Mi) of the weight mass, usable for momentcompensation of the i'th part, with respect to the targetcenter-of-gravity trajectory, and the modified acceleration (two orderdifferentiation of Δμ_(Mi) with respect to time) are unknown variables,is derived. In addition, an equation of motion pertinent to translation,indicated by the following equation (23), is derived. In the aboveequations, r_(Mi) is the position vector of the center of gravity of theweight mass M_(i) usable for moment compensation in the i'th part group.M _(i)(Δr _(M) _(i) −P _(zmp))×Δ{umlaut over (r)} _(M) _(i)+Ω_(i)=0  (22)M _(i) Δ{umlaut over (r)} _(M) _(i) +Γ_(i)=0  (23)

$\begin{matrix}{{{total}\mspace{14mu}{mass}\mspace{14mu}{points}\mspace{14mu}{usable}\mspace{14mu}{by}\mspace{14mu}{the}\mspace{14mu} i^{\prime}{th}\mspace{14mu}{site}\mspace{14mu}{for}\mspace{14mu}{moment}\mspace{14mu}{compensation}}\text{}{{{\sum\limits_{i}{{m_{i}\left( {{\Delta\; r_{i}} - r_{c}} \right)} \times \Delta\;{\overset{¨}{r}}_{i}}} + \Omega_{i}} = 0}} & (24)\end{matrix}$M _(i)(Δr _(M) _(i) −r _(c))×Δ{umlaut over (r)} _(M) _(i) +Ω_(i)=0  (25)

The equations, thus derived, now must be solved simultaneously. However,in the case of the legged mobile robot, in which the respective linksare interconnected by rotational joints, the above equations co-own themovements in the Z-direction, and hence represent non-linear two orderdifferential equations, presenting interferences, so that theseequations are difficult to solve analytically.

Thus, it is assumed that the equations do not co-own the motion in theZ-direction. The equation (23) is rendered non-linear and free ofinterferences, and an approximate solution r_(iMi) of the targetcenter-of-gravity trajectory of the weight mass usable for forcecompensation of the i'th part is calculated. The amount of change Δr_(i)of each mass point r_(i) of the i'th part, that may be calculated fromthe approximate solution, is substituted in the strict equation ofmotion (21) to calculate a force compensation error. This error is theninverted in sign and accumulated in the right side of the equation (23)previously rendered non-linear and free of interferences.

Then, r_(iMi) (known variable as calculated)−r_(Mi) (knownvariable)+Δr_(Mi) (unknown variable) is substituted for Δr_(Mi) (unknownvariable) and an approximate solution of the amount of change Δr_(Mi)with respect to the target center-of-gravity trajectory of the weightmass usable for moment compensation of the i'th part from the previouslycalculated target center-of-gravity trajectory is calculated.Specifically, the Z-axis motion is assumed to be not co-owned and theequation (22) or (25) is rendered non-linear and free of interferencesto calculate the approximate solution of the target center-of-gravitytrajectory of the weight mass usable for moment compensation of the i'thpart. The amount of change Δr_(i) of each mass point r_(i) of the i'thpart, that may be calculated from the approximate solution, issubstituted in the strict equation of motion (20) or in the aboveequation (24) to calculate a moment compensation error. This error isthen inverted in sign and accumulated in the right side of the equation(22) or (25) previously rendered non-linear and free of interferences.

An approximate solution of the target center-of-gravity trajectory ofthe weight mass usable for force compensation of the i'th part is againcalculated. The amount of change Δr_(i) of each mass point r_(i) of thei'th part, that may be calculated from the approximate solution, issubstituted in the strict equation of motion (21) to calculate a forcecompensation error. This error is then inverted in sign and accumulatedin the right side of the equation (23) previously rendered non-linearand free of interferences.

Then, r_(iMi) (known variable as calculated)−r_(Mi) (knownvariable)+Δr_(Mi) (unknown variable) is substituted for Δr_(Mi) (unknownvariable) and an approximate solution of the target center-of-gravitytrajectory of the weight mass usable for force compensation of the i'thpart is calculated. The amount of change Δr_(i) of each mass point r_(i)of the i'th part, that may be calculated from the approximate solution,is substituted in the strict equation of motion (20) or in the aboveequation (24) to calculate a moment compensation error. This error isthen inverted in sign and accumulated in the right side of the equation(22) or (25) previously rendered non-linear and free of interferences.

By repeating the above operations until the error becomes not largerthan an allowable value, it is possible to calculate the compensationmovement of the moment, in addition to the force of the part in question(step S85). In this manner, the target trajectory pertinent to the i'thpart as counted from the leading part in terms of the order of thepriority sequence may be obtained.

In general, it is sufficient to calculate the motion which satisfies theZMP equation or the motion of equation. However, there are occasionswhere the equations (20) and (21) must be satisfied simultaneously, suchas when the robot is in takeoff or in touchdown state in case of jumpingor running.

Meanwhile, in calculating the trajectory of each mass point of an i'thpart from the approximate solution of the target center-of-gravitytrajectory of the weight mass usable for moment compensation or forcecompensation of the i'th part, an operating point is set at an optionali'th part, a center-of-gravity vector C (X, Y, Z, θ_(x), θ_(y), θ_(z)),having the translational positions (X, Y, Z) or the rotational angles(θ_(x), θ_(y), θ_(z)) as unknown variables, is derived. An equation C(X, Y, Z, θ_(x), θ_(y), θ_(z))=r_(iMi) in which the center-of-gravityvector C (X, Y, Z, θ_(x), θ_(y), θ_(z)) is arranged as an unknownvariable on the left side and in which r_(Mi) is arranged as a knownvariable on the right side, is derived, and the solution of thisequation is calculated.

There are occasions where, depending on the degree-of-freedomconfiguration of the i'th part, the motion which satisfies the prioritysequence ρ (ρ_(p), ρ_(r), ρ_(yaw), ρ_(x), ρ_(y), ρ_(z)) does not exist,as when there is no degree of freedom of motion along the pitch axisdirection or along the roll axis direction suffering from interferenceagainst X-axis motion or Y-axis motion only to a lesser extent. In suchcase, the processing needs to revert to the step S61, S72 or S74 tore-set the relevant items.

Moreover, there are occasions where, depending on the posture of thei'th part, there is produced a marked difference in the error convergingrate in the iterative calculations of the respective elements of thepriority sequence ρ (ρ_(p), ρ_(r), ρ_(yaw), ρ_(x), ρ_(y), ρ_(z)) suchthat the strict solution of the equation of the element with a slowerror converging rate is not found and the strict solution of theequation of the element with a fast error converging rate is firstfound. In such case, the error in the amount of each moment compensationor the error in the amount of each force compensation, calculated in thestrict equation, needs to be multiplied with converging rate adjustmentcoefficients μ (μ_(p), μ_(r), μ_(yaw), μ_(x), μ_(y), μ_(z)) and theresults are accumulated in the right sides of the respective approximateequations for homogenizing the error converging rate.

The above processing is carried out for the totality of the part groupsto generate whole-body movements, such as walking.

[Supplement]

The present invention has so far been elucidated by referring toparticular embodiments thereof. It is however apparent that variouscorrections or substitutions of the above-described embodiments may bemade without departing form the scope of the invention.

The purport of the invention is not limited to a product termed a‘robot’. That is, the present invention is equally applicable tomechanical devices or other general mobile products performing themovement similar to the motion of human beings, using electrical ormagnetic actions, such as a toy.

In sum, the present invention has so far been disclosed from theperspective of illustration, and hence the scope of the presentinvention should be defined in light of the claims without beingconstrued in a limiting fashion.

INDUSTRIAL APPLICABILITY

According to the present invention, described above, there may beprovided a method and an apparatus optimum for controlling the operationof the robot apparatus, and a computer program, in which the targettrajectory for each part on the robot body may be corrected tocompensate the unknown external moment or unknown external force.

According to the present invention, there may be provided a method andan apparatus optimum for controlling the operation of the robotapparatus, and a computer program, in which the moment error applied tothe robot body about the roll, pitch and yaw axes during the robot'smovements, such as walking, may be canceled out with advantage byconcerted operations of the respective part groups making up the robotbody.

According to the present invention, there may be provided a method andan apparatus optimum for controlling the operation of the robotapparatus, and a computer program, in which moment errors applied to therobot body about the roll, pitch and yaw axes may be canceled out withadvantage even when the apparatus clears the floor surface, such asduring jumping or descending from an elevated place.

1. A motion controlling apparatus for a robot apparatus having at leasta plurality of movable parts, said motion controlling apparatuscomprising: ZMP (zero movement point) equilibrium equation generatingmeans for generating a ZMP equilibrium equation stating the relationshipof equilibrium of moments applied to a robot body of the robotapparatus; moment error calculating means for calculating moment errorsaccording to the ZMP equilibrium equation and target trajectories ofrespective parts of the robot body; priority sequence order settingmeans for setting the orders of the priority sequence of said respectiveparts, the target trajectories of which are to be corrected forcanceling out the moment error; and trajectory correction means forcorrecting the target trajectories of the respective parts in a sequenceprescribed by said priority sequence so set to compensate the momenterror.
 2. The motion controlling apparatus for a robot apparatusaccording to claim 1, wherein said priority sequence order setting meansaccords the orders of the priority sequence for correcting the targettrajectories to said parts in the sequence of the decreasing amount ofthe movable mass or in the sequence of the decreasing amount of thegeneratable moment.
 3. The motion controlling apparatus for a robotapparatus according to claim 1, wherein said priority sequence ordersetting means accords the orders of the priority sequence for correctingthe target trajectories to said parts by referring to a pre-draftedaction schedule.
 4. The motion controlling apparatus for a robotapparatus according to claim 1, wherein said ZMP equilibrium equationgenerating means derives a ZMP equation for each of the pitch, roll andyaw axes; said trajectory correction means correcting the targettrajectories of the respective parts for each of the pitch, roll and yawaxes to compensate the moment error.
 5. The motion controlling apparatusfor a robot apparatus according to claim 1, further comprising:center-of-gravity movement setting means for setting thecenter-of-gravity motion pertinent to translation and/or rotation ofsaid robot body when both foot units of said robot apparatus clear thefloor surface, wherein said moment error calculating means calculatesthe amount of force compensation in a target center of gravity, inaddition to the moment error in said ZMP equilibrium equation, whereinsaid priority sequence order setting means sets a priority sequence forsaid parts the target trajectories of which are corrected to cancel outthe moment error and the amount of force compensation, and wherein saidtrajectory correction means corrects the target trajectories for eachpart in a sequence corresponding to said priority sequence so set tocompensate the moment error and the amount of force compensation.
 6. Amotion controlling method for a robot apparatus having at least aplurality of movable parts, said motion controlling method comprising: aZMP equilibrium equation generating step of generating a ZMP equilibriumequation stating the relationship of equilibrium of moments applied to arobot body of the robot apparatus; a moment error calculating step ofcalculating moment errors according to the ZMP equilibrium equation andtarget trajectories of respective parts of the robot body; a prioritysequence order setting step of setting the orders of the prioritysequence of said respective parts, the target trajectories of which areto be corrected for canceling out the moment error; and a trajectorycorrection step of correcting the target trajectories of the respectiveparts in a sequence prescribed by said priority sequence so set tocompensate the moment error.
 7. The motion controlling method for arobot apparatus according to claim 6, wherein said priority sequenceorder setting step accords the orders of the priority sequence forcorrecting the target trajectories to said parts in the sequence of thedecreasing amount of the movable mass or in the sequence of thedecreasing amount of the generatable moment.
 8. The motion controllingmethod for a robot apparatus according to claim 6, wherein said prioritysequence order setting step accords the orders of the priority sequencefor correcting the target trajectories to said parts by referring to apre-drafted action schedule.
 9. The motion controlling method for arobot apparatus according to claim 6, wherein said ZMP equilibriumequation generating step derives a ZMP equation for each of the pitch,roll and yaw axes; said trajectory correction step correcting the targettrajectories of the respective parts for each of the pitch, roll and yawaxes to compensate the moment error.
 10. The motion controlling methodfor a robot apparatus according to claim 6 further comprising: acenter-of-gravity movement setting step of setting the center-of-gravitymotion pertinent to translation and/or rotation of said robot body whenboth foot units of said robot apparatus clear the floor surface, whereinsaid moment error calculating step calculating the amount of forcecompensation in a target center-of-gravity position, in addition to themoment error in said ZMP equilibrium equation, wherein said prioritysequence order setting step setting a priority sequence for said partsthe target trajectories of which are corrected to cancel out the momenterror and the amount of force compensation, and wherein said trajectorycorrection step correcting the target trajectories for each part in asequence corresponding to said priority sequence so set to compensatethe moment error and the amount of force compensation.
 11. A recordingmedium storing a computer program, which, when executed, allows theoperation control for a robot apparatus having at least a plurality ofmovable parts to be processed on a computer system, said programcomprising: a ZMP equilibrium equation generating step of generating aZMP equilibrium equation stating the relationship of equilibrium ofmoments applied to a robot body of the robot apparatus; a moment errorcalculating step of calculating moment errors according to the ZMPequilibrium equation and target trajectories of respective parts of therobot body; a priority sequence order setting step of setting the ordersof the priority sequence of said parts the target trajectories of whichare to be corrected for canceling out the moment error; and a trajectorycorrection step of correcting the target trajectories of the respectiveparts in a sequence prescribed by said priority sequence so set tocompensate the moment error.